#include "pid.h"

/// @brief 配置PID参数
/// @param pid PID控制器句柄
/// @param cfg PID控制器参数
void PidConfig(PidHandler *pid, const PidConfigType *cfg)
{
	pid->p0 = cfg->Kp + cfg->Ki + cfg->Kd;
	pid->p1 = -cfg->Kp - 2 * cfg->Kd;
	pid->p2 = cfg->Kd;

	pid->minOutput = cfg->minOutput;
	pid->maxOutput = cfg->maxOutput;
}

/// @brief 计算PID输出
/// @param pid PID控制器句柄
/// @param error PID环节输入，为目标值与当前值之间的误差
/// @return PID输出
PidCompuateType PidCompute(PidHandler *pid, PidCompuateType error)
{
	// 计算增量式PID输出
	pid->output += pid->p0 * error + pid->p1 * pid->prevError[0] + pid->p2 * pid->prevError[1];

	// 输出限制
	if (pid->output > pid->maxOutput)
	{
		pid->output = pid->maxOutput;
	}
	else if (pid->output < pid->minOutput)
	{
		pid->output = pid->minOutput;
	}

	// 更新上次误差
	pid->prevError[1] = pid->prevError[0];
	pid->prevError[0] = error;

	return pid->output;
}

/// @brief 重置PID控制器
/// @param pid PID控制器句柄
void PidReset(PidHandler *pid)
{
	pid->output = 0;
	pid->prevError[0] = 0;
	pid->prevError[1] = 0;
}
